// Copyright 2020 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <gmock/gmock.h>
#include <string>

#include "hardware_interface/component_parser.hpp"

using namespace ::testing;  // NOLINT

class TestComponentParser : public Test
{
protected:
  void SetUp() override
  {
    urdf_xml_head_ =
      R"(
<?xml version="1.0" encoding="utf-8"?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from minimal_robot.urdf.xacro       | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="MinimalRobot">
  <!--  <xacro:include filename="$(find ros2_control_demo_robot)/description/demo_2dof/robot_2dof.urdf.xacro" />
  <xacro:include filename="$(find ros2_control_demo_robot)/description/demo_2dof/robot_2dof.gazebo.xacro" />
  <xacro:include filename="$(find ros2_control_demo_robot)/description/demo_2dof/robot_2dof.ros2_control.xacro" />-->
  <!-- Used for fixing robot -->
  <link name="world"/>
  <gazebo reference="world">
    <static>true</static>
  </gazebo>
  <joint name="base_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="world"/>
    <child link="base_link"/>
  </joint>
  <link name="base_link">
    <inertial>
      <mass value="0.01"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.2" radius="0.1"/>
      </geometry>
      <material name="DarkGrey">
        <color rgba="0.4 0.4 0.4 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="1" radius="0.1"/>
      </geometry>
    </collision>
  </link>
  <joint name="joint1" type="revolute">
    <origin rpy="-1.57079632679 0 0" xyz="0 0 0.2"/>
    <parent link="base_link"/>
    <child link="link1"/>
    <limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
  </joint>
  <link name="link1">
    <inertial>
      <mass value="0.01"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="1" radius="0.1"/>
      </geometry>
      <material name="DarkGrey">
        <color rgba="0.4 0.4 0.4 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="1" radius="0.1"/>
      </geometry>
    </collision>
  </link>
  <joint name="joint2" type="revolute">
    <origin rpy="1.57079632679 0 0" xyz="0 0 0.9"/>
    <parent link="link1"/>
    <child link="link2"/>
    <limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
  </joint>
  <link name="link2">
    <inertial>
      <mass value="0.01"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="1" radius="0.1"/>
      </geometry>
      <material name="DarkGrey">
        <color rgba="0.4 0.4 0.4 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="1" radius="0.1"/>
      </geometry>
    </collision>
  </link>
  <joint name="tool_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 1"/>
    <parent link="link2"/>
    <child link="tool_link"/>
  </joint>
  <link name="tool_link">
    </link>
  <gazebo reference="base_link">
    <material>Gazebo/Gray</material>
  </gazebo>
  <gazebo reference="link1">
    <material>Gazebo/Gray</material>
  </gazebo>
  <gazebo reference="link2">
    <material>Gazebo/Gray</material>
  </gazebo>
  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
      </plugin>
  </gazebo>
)";

    urdf_xml_tail_ =
      R"(
</robot>
)";

// 1. Industrial Robots with only one interface
    valid_urdf_ros2_control_system_one_interface_ =
      R"(
  <ros2_control name="RRBotSystemPositionOnly" type="system">
    <hardware>
      <plugin>ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware</plugin>
      <param name="example_param_write_for_sec">2</param>
      <param name="example_param_read_for_sec">2</param>
    </hardware>
    <joint name="joint1">
      <command_interface name="position">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="position"/>
    </joint>
    <joint name="joint2">
      <command_interface name="position">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="position"/>
    </joint>
  </ros2_control>
)";

// 2. Industrial Robots with multiple interfaces (cannot be written at the same time)
// Note, joint parameters can be any string
    valid_urdf_ros2_control_system_multi_interface_ =
      R"(
  <ros2_control name="RRBotSystemMultiInterface" type="system">
    <hardware>
      <plugin>ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware</plugin>
      <param name="example_param_write_for_sec">2</param>
      <param name="example_param_read_for_sec">2</param>
    </hardware>
    <joint name="joint1">
      <command_interface name="position">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <command_interface name="effort">
        <param name="min">-0.5</param>
        <param name="max">0.5"</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="joint2">
      <command_interface name="position">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
  </ros2_control>
)";

// 3. Industrial Robots with integrated sensor
    valid_urdf_ros2_control_system_robot_with_sensor_ =
      R"(
  <ros2_control name="RRBotSystemWithSensor" type="system">
    <hardware>
      <plugin>ros2_control_demo_hardware/RRBotSystemWithSensorHardware</plugin>
      <param name="example_param_write_for_sec">2</param>
      <param name="example_param_read_for_sec">2</param>
    </hardware>
    <joint name="joint1">
      <command_interface name="position">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="position"/>
    </joint>
    <joint name="joint2">
      <command_interface name="position">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="position"/>
    </joint>
    <sensor name="tcp_fts_sensor">
      <state_interface name="fx"/>
      <state_interface name="fy"/>
      <state_interface name="fz"/>
      <state_interface name="tx"/>
      <state_interface name="ty"/>
      <state_interface name="tz"/>
      <param name="frame_id">kuka_tcp</param>
      <param name="lower_limits">-100</param>
      <param name="upper_limits">100</param>
    </sensor>
  </ros2_control>
)";

// 4. Industrial Robots with externally connected sensor
    valid_urdf_ros2_control_system_robot_with_external_sensor_ =
      R"(
  <ros2_control name="RRBotSystemPositionOnlyWithExternalSensor" type="system">
    <hardware>
      <plugin>ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware</plugin>
      <param name="example_param_write_for_sec">2</param>
      <param name="example_param_read_for_sec">2</param>
    </hardware>
    <joint name="joint1">
      <command_interface name="position">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="position"/>
    </joint>
    <joint name="joint2">
      <command_interface name="position">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="position"/>
    </joint>
  </ros2_control>
  <ros2_control name="RRBotForceTorqueSensor2D" type="sensor">
    <hardware>
      <plugin>ros2_control_demo_hardware/ForceTorqueSensor2DHardware</plugin>
      <param name="example_param_read_for_sec">0.43</param>
    </hardware>
    <sensor name="tcp_fts_sensor">
      <state_interface name="fx"/>
      <state_interface name="fy"/>
      <state_interface name="fz"/>
      <state_interface name="tx"/>
      <state_interface name="ty"/>
      <state_interface name="tz"/>
      <param name="frame_id">kuka_tcp</param>
      <param name="lower_limits">-100</param>
      <param name="upper_limits">100</param>
    </sensor>
  </ros2_control>
)";

// 5. Modular Robots with separate communication to each actuator
    valid_urdf_ros2_control_actuator_modular_robot_ =
      R"(
  <ros2_control name="RRBotModularJoint1"  type="actuator">
    <hardware>
      <plugin>ros2_control_demo_hardware/PositionActuatorHardware</plugin>
      <param name="example_param_write_for_sec">1.23</param>
      <param name="example_param_read_for_sec">3</param>
    </hardware>
    <joint name="joint1">
      <command_interface name="position">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="position"/>
    </joint>
  </ros2_control>
  <ros2_control name="RRBotModularJoint2"  type="actuator">
    <hardware>
      <plugin>ros2_control_demo_hardware/PositionActuatorHardware</plugin>
      <param name="example_param_write_for_sec">1.23</param>
      <param name="example_param_read_for_sec">3</param>
    </hardware>
    <joint name="joint2">
      <command_interface name="position">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="position"/>
    </joint>
  </ros2_control>
)";

// 6. Modular Robots with actuators not providing states and with additional sensors
// Example for simple transmission
    valid_urdf_ros2_control_actuator_modular_robot_sensors_ =
      R"(
  <ros2_control name="RRBotModularJoint1" type="actuator">
    <hardware>
      <plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
      <param name="example_param_write_for_sec">1.23</param>
      <param name="example_param_read_for_sec">3</param>
    </hardware>
    <joint name="joint1">
      <command_interface name="velocity">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="velocity"/>
    </joint>
    <transmission name="transmission1">
      <plugin>transmission_interface/SimpleTansmission</plugin>
      <param name="joint_to_actuator">${1024/PI}</param>
    </transmission>
  </ros2_control>
  <ros2_control name="RRBotModularJoint2" type="actuator">
    <hardware>
      <plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
      <param name="example_param_write_for_sec">1.23</param>
      <param name="example_param_read_for_sec">3</param>
    </hardware>
    <joint name="joint2">
      <command_interface name="velocity">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="velocity"/>
    </joint>
  </ros2_control>
  <ros2_control name="RRBotModularPositionSensorJoint1" type="sensor">
    <hardware>
      <plugin>ros2_control_demo_hardware/PositionSensorHardware</plugin>
      <param name="example_param_read_for_sec">2</param>
    </hardware>
    <joint name="joint1">
      <state_interface name="position"/>
    </joint>
  </ros2_control>
  <ros2_control name="RRBotModularPositionSensorJoint2" type="sensor">
    <hardware>
      <plugin>ros2_control_demo_hardware/PositionSensorHardware</plugin>
      <param name="example_param_read_for_sec">2</param>
    </hardware>
    <joint name="joint2">
      <state_interface name="position"/>
    </joint>
  </ros2_control>
)";

// 7. Modular Robots with separate communication to each "actuator" with multi joints
// Example for complex transmission (multi-joint/multi-actuator) - (system component is used)
    valid_urdf_ros2_control_system_muti_joints_transmission_ =
      R"(
  <ros2_control name="RRBotModularWrist" type="system">
    <hardware>
      <plugin>ros2_control_demo_hardware/ActuatorHardwareMultiDOF</plugin>
      <param name="example_param_write_for_sec">1.23</param>
      <param name="example_param_read_for_sec">3</param>
    </hardware>
    <joint name="joint1">
      <command_interface name="position">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="position"/>
    </joint>
    <joint name="joint2">
      <command_interface name="position">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="position"/>
    </joint>
    <transmission name="transmission1">
      <plugin>transmission_interface/SomeComplex2by2Transmission</plugin>
      <param name="joints">{joint1, joint2}</param>
      <param name="output">{output2, output2}</param>
      <param name="joint1_output1">1.5</param>
      <param name="joint1_output2">3.2</param>
      <param name="joint2_output1">3.1</param>
      <param name="joint2_output2">1.4</param>
    </transmission>
  </ros2_control>
)";

// 8. Sensor only
    valid_urdf_ros2_control_sensor_only_ =
      R"(
  <ros2_control name="CameraWithIMU" type="sensor">
    <hardware>
      <plugin>ros2_control_demo_hardware/CameraWithIMUSensor</plugin>
      <param name="example_param_read_for_sec">2</param>
    </hardware>
    <sensor name="sensor1">
      <state_interface name="roll"/>
      <state_interface name="pitch"/>
      <state_interface name="yaw"/>
    </sensor>
    <sensor name="sensor2">
      <state_interface name="image"/>
    </sensor>
  </ros2_control>
)";

// 9. Actuator Only
    valid_urdf_ros2_control_actuator_only_ =
      R"(
  <ros2_control name="ActuatorModularJoint1" type="actuator">
    <hardware>
      <plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
      <param name="example_param_write_for_sec">1.13</param>
      <param name="example_param_read_for_sec">3</param>
    </hardware>
    <joint name="joint1">
      <command_interface name="velocity">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="velocity"/>
    </joint>
    <transmission name="transmission1">
      <plugin>transmission_interface/RotationToLinerTansmission</plugin>
      <param name="joint_to_actuator">${1024/PI}</param>
    </transmission>
  </ros2_control>
)";

// Errors
    invalid_urdf_ros2_control_invalid_child_ =
      R"(
  <ros2_control name="2DOF_System_Robot_Position_Only" type="system">
    <hardwarert>
      <plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
      <param name="example_param_write_for_sec">2</param>
      <param name="example_param_read_for_sec">2</param>
    </hardwarert>
  </ros2_control>
  )";

    invalid_urdf_ros2_control_missing_attribute_ =
      R"(
  <ros2_control type="system">
    <hardware>
      <plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
      <param name="example_param_write_for_sec">2</param>
      <param name="example_param_read_for_sec">2</param>
    </hardware>
  </ros2_control>
  )";

    invalid_urdf_ros2_control_component_missing_class_type_ =
      R"(
  <ros2_control name="2DOF_System_Robot_Position_Only" type="system">
    <hardware>
      <plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
      <param name="example_param_write_for_sec">2</param>
      <param name="example_param_read_for_sec">2</param>
    </hardware>
    <joint name="joint1">
      <param name="min_position_value">-1</param>
      <param name="max_position_value">1</param>
    </joint>
  </ros2_control>
)";

    invalid_urdf_ros2_control_parameter_missing_name_ =
      R"(
  <ros2_control name="2DOF_System_Robot_Position_Only" type="system">
    <hardware>
      <plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
      <param>2</param>
      <param name="example_param_read_for_sec">2</param>
    </hardware>
    <joint name="joint1">
      <plugin>ros2_control_components/PositionJoint</plugin>
      <param name="min_position_value">-1</param>
      <param name="max_position_value">1</param>
    </joint>
  </ros2_control>
)";


    invalid_urdf_ros2_control_component_class_type_empty_ =
      R"(
  <ros2_control name="2DOF_System_Robot_Position_Only" type="system">
    <hardware>
      <plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
      <param name="example_param_write_for_sec">2</param>
      <param name="example_param_read_for_sec">2</param>
    </hardware>
    <joint name="joint1">
      <plugin></plugin>
      <param name="min_position_value">-1</param>
      <param name="max_position_value">1</param>
    </joint>
  </ros2_control>
)";

    invalid_urdf_ros2_control_component_interface_type_empty_ =
      R"(
  <ros2_control name="2DOF_System_Robot_Position_Only" type="system">
    <hardware>
      <plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
      <param name="example_param_write_for_sec">2</param>
      <param name="example_param_read_for_sec">2</param>
    </hardware>
    <joint name="joint1">
      <plugin>ros2_control_components/PositionJoint</plugin>
      <state_interface></state_interface>
      <param name="min_position_value">-1</param>
      <param name="max_position_value">1</param>
    </joint>
  </ros2_control>
)";

    invalid_urdf_ros2_control_parameter_empty_ =
      R"(
  <ros2_control name="2DOF_System_Robot_Position_Only" type="system">
    <hardware>
      <plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
      <param name="example_param_write_for_sec"></param>
      <param name="example_param_read_for_sec">2</param>
    </hardware>
    <joint name="joint1">
      <command_interface name="position">
        <param name="min_position_value">-1</param>
        <param name="max_position_value">1</param>
      </command_interface>
    </joint>
  </ros2_control>
)";
  }

  std::string urdf_xml_head_, urdf_xml_tail_;
  std::string valid_urdf_ros2_control_system_one_interface_;
  std::string valid_urdf_ros2_control_system_multi_interface_;
  std::string valid_urdf_ros2_control_system_robot_with_sensor_;
  std::string valid_urdf_ros2_control_system_robot_with_external_sensor_;
  std::string valid_urdf_ros2_control_actuator_modular_robot_;
  std::string valid_urdf_ros2_control_actuator_modular_robot_sensors_;
  std::string valid_urdf_ros2_control_system_muti_joints_transmission_;
  std::string valid_urdf_ros2_control_sensor_only_;
  std::string valid_urdf_ros2_control_actuator_only_;

  std::string invalid_urdf_ros2_control_invalid_child_;
  std::string invalid_urdf_ros2_control_missing_attribute_;
  std::string invalid_urdf_ros2_control_component_missing_class_type_;
  std::string invalid_urdf_ros2_control_component_class_type_empty_;
  std::string invalid_urdf_ros2_control_component_interface_type_empty_;
  std::string invalid_urdf_ros2_control_parameter_missing_name_;
  std::string invalid_urdf_ros2_control_parameter_empty_;
};

using hardware_interface::parse_control_resources_from_urdf;

TEST_F(TestComponentParser, empty_string_throws_error)
{
  ASSERT_THROW(parse_control_resources_from_urdf(""), std::runtime_error);
}

TEST_F(TestComponentParser, empty_urdf_throws_error)
{
  const std::string empty_urdf =
    "<?xml version=\"1.0\"?><robot name=\"robot\" xmlns=\"http://www.ros.org\"></robot>";

  ASSERT_THROW(parse_control_resources_from_urdf(empty_urdf), std::runtime_error);
}

TEST_F(TestComponentParser, string_robot_not_root_throws_error)
{
  const std::string broken_xml_string =
    R"(
    <?xml version=\"1.0\"?><ros2_control name=\"robot\">><robot name=\"robot\" xmlns=\"http://www.ros.org\"></robot></ros2_control>
    )";

  ASSERT_THROW(parse_control_resources_from_urdf(broken_xml_string), std::runtime_error);
}

TEST_F(TestComponentParser, invalid_child_throws_error)
{
  const std::string broken_urdf_string = urdf_xml_head_ + invalid_urdf_ros2_control_invalid_child_ +
    urdf_xml_tail_;

  ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error);
}

TEST_F(TestComponentParser, missing_attribute_throws_error)
{
  const std::string broken_urdf_string = urdf_xml_head_ +
    invalid_urdf_ros2_control_missing_attribute_ + urdf_xml_tail_;

  ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error);
}

TEST_F(TestComponentParser, parameter_missing_name_throws_error)
{
  const std::string broken_urdf_string = urdf_xml_head_ +
    invalid_urdf_ros2_control_parameter_missing_name_ + urdf_xml_tail_;

  ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error);
}

TEST_F(TestComponentParser, component_interface_type_empty_throws_error)
{
  const std::string broken_urdf_string = urdf_xml_head_ +
    invalid_urdf_ros2_control_component_interface_type_empty_ + urdf_xml_tail_;

  ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error);
}

TEST_F(TestComponentParser, parameter_empty_throws_error)
{
  const std::string broken_urdf_string = urdf_xml_head_ +
    invalid_urdf_ros2_control_parameter_empty_ + urdf_xml_tail_;

  ASSERT_THROW(parse_control_resources_from_urdf(broken_urdf_string), std::runtime_error);
}

TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_one_interface)
{
  std::string urdf_to_test = urdf_xml_head_ + valid_urdf_ros2_control_system_one_interface_ +
    urdf_xml_tail_;
  const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
  ASSERT_THAT(control_hardware, SizeIs(1));
  const auto hardware_info = control_hardware.front();

  EXPECT_EQ(hardware_info.name, "RRBotSystemPositionOnly");
  EXPECT_EQ(hardware_info.type, "system");
  EXPECT_EQ(
    hardware_info.hardware_class_type,
    "ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware");
  ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2));
  EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2");

  ASSERT_THAT(hardware_info.joints, SizeIs(2));

  EXPECT_EQ(hardware_info.joints[0].name, "joint1");
  EXPECT_EQ(hardware_info.joints[0].type, "joint");
  ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1));
  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, "position");
  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "-1");
  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "1");
  ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1));
  EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, "position");

  EXPECT_EQ(hardware_info.joints[1].name, "joint2");
  EXPECT_EQ(hardware_info.joints[1].type, "joint");
  ASSERT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(1));
  EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, "position");
  EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].min, "-1");
  EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].max, "1");
  ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(1));
  EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, "position");
}

TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface)
{
  std::string urdf_to_test = urdf_xml_head_ + valid_urdf_ros2_control_system_multi_interface_ +
    urdf_xml_tail_;
  const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
  ASSERT_THAT(control_hardware, SizeIs(1));
  const auto hardware_info = control_hardware.front();

  EXPECT_EQ(hardware_info.name, "RRBotSystemMultiInterface");
  EXPECT_EQ(hardware_info.type, "system");
  EXPECT_EQ(
    hardware_info.hardware_class_type,
    "ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware");
  ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2));
  EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2");
  EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2");

  ASSERT_THAT(hardware_info.joints, SizeIs(2));

  EXPECT_EQ(hardware_info.joints[0].name, "joint1");
  EXPECT_EQ(hardware_info.joints[0].type, "joint");
  ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(3));
  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, "position");
  ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(3));
  EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].name, "velocity");

  EXPECT_EQ(hardware_info.joints[1].name, "joint2");
  EXPECT_EQ(hardware_info.joints[1].type, "joint");
  ASSERT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(1));
  ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(3));
  EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].name, "effort");
}

TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sensor)
{
  std::string urdf_to_test = urdf_xml_head_ + valid_urdf_ros2_control_system_robot_with_sensor_ +
    urdf_xml_tail_;
  const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
  ASSERT_THAT(control_hardware, SizeIs(1));
  const auto hardware_info = control_hardware.front();

  EXPECT_EQ(hardware_info.name, "RRBotSystemWithSensor");
  EXPECT_EQ(hardware_info.type, "system");
  EXPECT_EQ(
    hardware_info.hardware_class_type,
    "ros2_control_demo_hardware/RRBotSystemWithSensorHardware");
  ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2));
  EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2");

  ASSERT_THAT(hardware_info.joints, SizeIs(2));

  EXPECT_EQ(hardware_info.joints[0].name, "joint1");
  EXPECT_EQ(hardware_info.joints[0].type, "joint");

  EXPECT_EQ(hardware_info.joints[1].name, "joint2");
  EXPECT_EQ(hardware_info.joints[1].type, "joint");

  ASSERT_THAT(hardware_info.sensors, SizeIs(1));

  EXPECT_EQ(hardware_info.sensors[0].name, "tcp_fts_sensor");
  EXPECT_EQ(hardware_info.sensors[0].type, "sensor");
  EXPECT_THAT(hardware_info.sensors[0].state_interfaces, SizeIs(6));
  EXPECT_THAT(hardware_info.sensors[0].command_interfaces, IsEmpty());
  EXPECT_THAT(hardware_info.sensors[0].state_interfaces[0].name, "fx");
  EXPECT_THAT(hardware_info.sensors[0].state_interfaces[1].name, "fy");
  EXPECT_THAT(hardware_info.sensors[0].state_interfaces[2].name, "fz");
  EXPECT_THAT(hardware_info.sensors[0].state_interfaces[3].name, "tx");
  EXPECT_THAT(hardware_info.sensors[0].state_interfaces[4].name, "ty");
  EXPECT_THAT(hardware_info.sensors[0].state_interfaces[5].name, "tz");

  ASSERT_THAT(hardware_info.sensors[0].parameters, SizeIs(3));
  EXPECT_EQ(hardware_info.sensors[0].parameters.at("frame_id"), "kuka_tcp");
  EXPECT_EQ(hardware_info.sensors[0].parameters.at("lower_limits"), "-100");
  EXPECT_EQ(hardware_info.sensors[0].parameters.at("upper_limits"), "100");
}

TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_external_sensor)
{
  std::string urdf_to_test = urdf_xml_head_ +
    valid_urdf_ros2_control_system_robot_with_external_sensor_ + urdf_xml_tail_;
  const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
  ASSERT_THAT(control_hardware, SizeIs(2));
  auto hardware_info = control_hardware.at(0);

  EXPECT_EQ(hardware_info.name, "RRBotSystemPositionOnlyWithExternalSensor");
  EXPECT_EQ(hardware_info.type, "system");
  EXPECT_EQ(
    hardware_info.hardware_class_type,
    "ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware");
  ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2));
  EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2");

  ASSERT_THAT(hardware_info.joints, SizeIs(2));

  EXPECT_EQ(hardware_info.joints[0].name, "joint1");
  EXPECT_EQ(hardware_info.joints[0].type, "joint");

  EXPECT_EQ(hardware_info.joints[1].name, "joint2");
  EXPECT_EQ(hardware_info.joints[1].type, "joint");

  ASSERT_THAT(hardware_info.sensors, SizeIs(0));

  hardware_info = control_hardware.at(1);

  EXPECT_EQ(hardware_info.name, "RRBotForceTorqueSensor2D");
  EXPECT_EQ(hardware_info.type, "sensor");
  ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(1));
  EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "0.43");

  ASSERT_THAT(hardware_info.sensors, SizeIs(1));
  EXPECT_EQ(hardware_info.sensors[0].name, "tcp_fts_sensor");
  EXPECT_EQ(hardware_info.sensors[0].type, "sensor");
  EXPECT_EQ(hardware_info.sensors[0].parameters.at("frame_id"), "kuka_tcp");
}

TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot)
{
  std::string urdf_to_test = urdf_xml_head_ + valid_urdf_ros2_control_actuator_modular_robot_ +
    urdf_xml_tail_;
  const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
  ASSERT_THAT(control_hardware, SizeIs(2));
  auto hardware_info = control_hardware.at(0);

  EXPECT_EQ(hardware_info.name, "RRBotModularJoint1");
  EXPECT_EQ(hardware_info.type, "actuator");
  EXPECT_EQ(
    hardware_info.hardware_class_type,
    "ros2_control_demo_hardware/PositionActuatorHardware");
  ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2));
  EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.23");

  ASSERT_THAT(hardware_info.joints, SizeIs(1));
  EXPECT_EQ(hardware_info.joints[0].name, "joint1");
  EXPECT_EQ(hardware_info.joints[0].type, "joint");

  hardware_info = control_hardware.at(1);

  EXPECT_EQ(hardware_info.name, "RRBotModularJoint2");
  EXPECT_EQ(hardware_info.type, "actuator");
  EXPECT_EQ(
    hardware_info.hardware_class_type,
    "ros2_control_demo_hardware/PositionActuatorHardware");
  ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2));
  EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "3");

  ASSERT_THAT(hardware_info.joints, SizeIs(1));
  EXPECT_EQ(hardware_info.joints[0].name, "joint2");
  EXPECT_EQ(hardware_info.joints[0].type, "joint");
}

TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot_with_sensors)
{
  std::string urdf_to_test = urdf_xml_head_ +
    valid_urdf_ros2_control_actuator_modular_robot_sensors_ + urdf_xml_tail_;
  const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
  ASSERT_THAT(control_hardware, SizeIs(4));
  auto hardware_info = control_hardware.at(0);

  EXPECT_EQ(hardware_info.name, "RRBotModularJoint1");
  EXPECT_EQ(hardware_info.type, "actuator");
  EXPECT_EQ(
    hardware_info.hardware_class_type,
    "ros2_control_demo_hardware/VelocityActuatorHardware");
  ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2));
  EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.23");

  ASSERT_THAT(hardware_info.joints, SizeIs(1));
  EXPECT_EQ(hardware_info.joints[0].name, "joint1");
  EXPECT_EQ(hardware_info.joints[0].type, "joint");
  ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1));
  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, "velocity");

  ASSERT_THAT(hardware_info.transmissions, SizeIs(1));
  EXPECT_EQ(hardware_info.transmissions[0].name, "transmission1");
  EXPECT_EQ(hardware_info.transmissions[0].type, "transmission");
// EXPECT_EQ(hardware_info.transmissions[0].class_type, "transmission_interface/SimpleTansmission");
  ASSERT_THAT(hardware_info.transmissions[0].parameters, SizeIs(1));
  EXPECT_EQ(hardware_info.transmissions[0].parameters.at("joint_to_actuator"), "${1024/PI}");

  hardware_info = control_hardware.at(1);

  EXPECT_EQ(hardware_info.name, "RRBotModularJoint2");
  EXPECT_EQ(hardware_info.type, "actuator");
  EXPECT_EQ(
    hardware_info.hardware_class_type,
    "ros2_control_demo_hardware/VelocityActuatorHardware");
  ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2));
  EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "3");

  ASSERT_THAT(hardware_info.joints, SizeIs(1));
  EXPECT_EQ(hardware_info.joints[0].name, "joint2");
  EXPECT_EQ(hardware_info.joints[0].type, "joint");
  ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1));
  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, "velocity");
  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "-1");
  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "1");

  hardware_info = control_hardware.at(2);

  EXPECT_EQ(hardware_info.name, "RRBotModularPositionSensorJoint1");
  EXPECT_EQ(hardware_info.type, "sensor");
  EXPECT_EQ(
    hardware_info.hardware_class_type,
    "ros2_control_demo_hardware/PositionSensorHardware");
  ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(1));
  EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2");

  ASSERT_THAT(hardware_info.sensors, SizeIs(0));
  ASSERT_THAT(hardware_info.joints, SizeIs(1));
  EXPECT_EQ(hardware_info.joints[0].name, "joint1");
  EXPECT_EQ(hardware_info.joints[0].type, "joint");
  ASSERT_THAT(hardware_info.joints[0].command_interfaces, IsEmpty());
  ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1));
  EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, "position");


  hardware_info = control_hardware.at(3);

  EXPECT_EQ(hardware_info.name, "RRBotModularPositionSensorJoint2");
  EXPECT_EQ(hardware_info.type, "sensor");
  EXPECT_EQ(hardware_info.hardware_class_type, "ros2_control_demo_hardware/PositionSensorHardware");
  ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(1));
  EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2");

  ASSERT_THAT(hardware_info.sensors, SizeIs(0));
  ASSERT_THAT(hardware_info.joints, SizeIs(1));
  EXPECT_EQ(hardware_info.joints[0].name, "joint2");
  EXPECT_EQ(hardware_info.joints[0].type, "joint");
  ASSERT_THAT(hardware_info.joints[0].command_interfaces, IsEmpty());
  ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1));
  EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, "position");
}

TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_joints_transmission)
{
  std::string urdf_to_test = urdf_xml_head_ +
    valid_urdf_ros2_control_system_muti_joints_transmission_ + urdf_xml_tail_;
  const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
  ASSERT_THAT(control_hardware, SizeIs(1));
  auto hardware_info = control_hardware.at(0);

  EXPECT_EQ(hardware_info.name, "RRBotModularWrist");
  EXPECT_EQ(hardware_info.type, "system");
  EXPECT_EQ(
    hardware_info.hardware_class_type,
    "ros2_control_demo_hardware/ActuatorHardwareMultiDOF");
  ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2));
  EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.23");

  ASSERT_THAT(hardware_info.joints, SizeIs(2));
  EXPECT_EQ(hardware_info.joints[0].name, "joint1");
  EXPECT_EQ(hardware_info.joints[0].type, "joint");
  EXPECT_EQ(hardware_info.joints[1].name, "joint2");
  EXPECT_EQ(hardware_info.joints[1].type, "joint");

  ASSERT_THAT(hardware_info.transmissions, SizeIs(1));
  EXPECT_EQ(hardware_info.transmissions[0].name, "transmission1");
  EXPECT_EQ(hardware_info.transmissions[0].type, "transmission");
  // EXPECT_EQ(
  //   hardware_info.transmissions[0].class_type,
  //   "transmission_interface/SomeComplex_2x2_Transmission");
  ASSERT_THAT(hardware_info.transmissions[0].parameters, SizeIs(6));
  EXPECT_EQ(hardware_info.transmissions[0].parameters.at("joint1_output1"), "1.5");
  EXPECT_EQ(hardware_info.transmissions[0].parameters.at("joint1_output2"), "3.2");
  EXPECT_EQ(hardware_info.transmissions[0].parameters.at("joint2_output1"), "3.1");
  EXPECT_EQ(hardware_info.transmissions[0].parameters.at("joint2_output2"), "1.4");
}

TEST_F(TestComponentParser, successfully_parse_valid_urdf_sensor_only)
{
  std::string urdf_to_test = urdf_xml_head_ + valid_urdf_ros2_control_sensor_only_ + urdf_xml_tail_;
  const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
  ASSERT_THAT(control_hardware, SizeIs(1));
  auto hardware_info = control_hardware.at(0);

  EXPECT_EQ(hardware_info.name, "CameraWithIMU");
  EXPECT_EQ(hardware_info.type, "sensor");
  EXPECT_EQ(hardware_info.hardware_class_type, "ros2_control_demo_hardware/CameraWithIMUSensor");
  ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(1));
  EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2");

  ASSERT_THAT(hardware_info.sensors, SizeIs(2));
  EXPECT_EQ(hardware_info.sensors[0].name, "sensor1");
  EXPECT_EQ(hardware_info.sensors[0].type, "sensor");
  ASSERT_THAT(hardware_info.sensors[0].state_interfaces, SizeIs(3));
  EXPECT_EQ(hardware_info.sensors[0].state_interfaces[0].name, "roll");
  EXPECT_EQ(hardware_info.sensors[0].state_interfaces[1].name, "pitch");
  EXPECT_EQ(hardware_info.sensors[0].state_interfaces[2].name, "yaw");

  EXPECT_EQ(hardware_info.sensors[1].name, "sensor2");
  EXPECT_EQ(hardware_info.sensors[1].type, "sensor");
  ASSERT_THAT(hardware_info.sensors[1].state_interfaces, SizeIs(1));
  EXPECT_EQ(hardware_info.sensors[1].state_interfaces[0].name, "image");
}

TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only)
{
  std::string urdf_to_test = urdf_xml_head_ +
    valid_urdf_ros2_control_actuator_only_ + urdf_xml_tail_;
  const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
  ASSERT_THAT(control_hardware, SizeIs(1));
  auto hardware_info = control_hardware.at(0);

  EXPECT_EQ(hardware_info.name, "ActuatorModularJoint1");
  EXPECT_EQ(hardware_info.type, "actuator");
  EXPECT_EQ(
    hardware_info.hardware_class_type,
    "ros2_control_demo_hardware/VelocityActuatorHardware");
  ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2));
  EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.13");

  ASSERT_THAT(hardware_info.joints, SizeIs(1));
  EXPECT_EQ(hardware_info.joints[0].name, "joint1");
  EXPECT_EQ(hardware_info.joints[0].type, "joint");
  ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1));
  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, "velocity");
  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "-1");
  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "1");
  ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1));
  EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, "velocity");


  ASSERT_THAT(hardware_info.transmissions, SizeIs(1));
  EXPECT_EQ(hardware_info.transmissions[0].name, "transmission1");
  EXPECT_EQ(hardware_info.transmissions[0].type, "transmission");
  // EXPECT_EQ(
  //   hardware_info.transmissions[0].class_type,
  //   "transmission_interface/RotationToLinerTansmission");
  ASSERT_THAT(hardware_info.transmissions[0].parameters, SizeIs(1));
  EXPECT_EQ(hardware_info.transmissions[0].parameters.at("joint_to_actuator"), "${1024/PI}");
}
